//find_toe_off.hpp
#include "behaviortree_cpp/action_node.h"
#include "utils/load_data.hpp"
// #include "utils/timer_utils.hpp"
// ANSI 颜色代码
#define COLOR_RED     "\033[31m"
#define COLOR_GREEN   "\033[32m"
#define COLOR_YELLOW  "\033[33m"
#define COLOR_BLUE    "\033[34m"
#define COLOR_MAGENTA "\033[35m"
#define COLOR_CYAN    "\033[36m"
#define COLOR_RESET   "\033[0m"    

class FindToeOff : public BT::StatefulActionNode
{
    public:
    explicit FindToeOff(const std::string &name,  const BT::NodeConfig& config) : BT::StatefulActionNode(name, config){cnt = 0;};
            BT::NodeStatus onStart() override; 
            BT::NodeStatus onRunning() override ;

            void onHalted() override{
                //处理中断
                std::cout<<"GripperClose_onHalted"<<std::endl;
            }
            // 添加静态端口声明方法
            static BT::PortsList providedPorts() {
                return BT::PortsList({
                    BT::InputPort<sensorData>("sensor_input", "Description of input"),
                    BT::InputPort<uint64_t>("T_hs", "Description of input"),
                    BT::BidirectionalPort<std::shared_ptr<std::vector<uint64_t>> >("mean_stance_duration", "Description of input"),
                    BT::OutputPort<uint64_t>("T_to", "time point of toe off")
                });
            }
    private:
            int cnt;
            uint64_t current_millis_=0;
            sensorData tmp_sensor_data_filtered_;
            double tof_distance_= 0;
            double tof_distance_last_= 0;
            uint64_t last_stance_duration_= 0.0;
            uint64_t stance_duration_= 0.0;
            std::vector<uint64_t> mean_stance_duration_ = {};
            std::shared_ptr<std::vector<uint64_t>> mean_stance_duration_ptr = nullptr;
            uint64_t T_hs_ = 0.0;
            uint64_t T_to_ = 0.0;
            void print_all_outports(bool print_switch);
            void print_all_inports(bool print_switch);
            void print_vector(const std::vector<uint64_t>& vec, const std::string& name);
            void print_var(const uint64_t& var, const std::string& name);
}; // 确保这里有分号

